#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import sys
import re
import os

import matplotlib.pyplot as plt

import logging
from loggercfg import loggerCfgConsoleHandler
from loggercfg import loggerCfgFileHandler
from loggercfg import loggerCfgFileAndConsoleHandler

loggerc = loggerCfgConsoleHandler()
loggerf = loggerCfgFileHandler()
loggerfc = loggerCfgFileAndConsoleHandler()


import threading
from pid import pid

'''
* functionname : stepMotor
* para()       :
* return       : 
* discription  : 
步进电机 1600转, 对应一圈, 对应1cm
10mS, 最大变速100转
* author       : zhongmingsen
* date         : 2023-06-14 
'''
class stepMotor(pid):
    def __init__(self, Kp, Ki, Kd, limitTarget, limitOut, limitKiErr) :
        self.deltaSrcDstMax = 0 #时间耗尽时，距离最大偏差
        self.p_src = 0         # 步进电机原始位置, 也指当前位置
        self.p_dst = 0         # 步进电机目标位置
        self.time = 0.0        # 步进电机从原始位置到达目标位置的时间
        
        self.ts = 0.01         # 步进电机调节时间，单位秒
        self.feedbackFreq = 0  # 步进电机真实运行速度，self.ts时间内的PWM数
        self.targetFreq = 0    # 步进电机目标运行速度，self.ts时间内的PWM数

        self.feedbackFreqPre = 0  # 步进电机真实运行速度，self.ts时间内的PWM数
        self.feedbackFreqMax = 0  # 步进电机真实运行速度，self.ts时间内的PWM数

        self.remainingDistanceMax = 0 #设置时间过短，最大速度下不能到达指定位置，用于PID调节添加时间的判断条件
        # positionalPidPara 实例化一个PID，并配置Kp,Ki,Kd,这三个参数要根据实际应用调节
        '''
        self.Kp = Kp  #比例系数
        self.Ki = Ki  #微分系数
        self.Kd = Kd  #积分系数

        self.limitTarget = limitTarget    #输出目标限制
        self.limitOut = limitOut       #输出调节量限制
        self.limitKiErr = limitKiErr  #输出积分调节量限制
        '''
        self.pidm = pid(Kp, Ki, Kd)
        self.pidm.setValue(limitTarget, limitOut, limitKiErr)  #设置PID的主要参数

        #self.pidm = pid(0.9, 0.01, 0.1)
        #self.pidm.setValue(300, 5, 15) 

        #用于仿真调试
        self.feedbackFreqList = []
        self.targetFreqList = []
        self.srcDisList = []
        self.dstDisList = []
        self.timeList = []
        self.remainingDistanceList = []
        self.remainingDistanceMaxList = []

    '''
    * functionname : stepMotor
    * para()       :
    * return       : 
    * discription  : 
    步进电机 1600转, 对应一圈, 对应1cm
    10mS, 最大变速100转
    * author       : zhongmingsen
    * date         : 2023-06-14 
    '''
    def stepMotorPara(self, src, dst, time) :
        if time < 0.0 :
            time = 0.0
        self.p_src = src     # 步进电机原始位置, 也指当前位置
        self.p_dst = dst     # 步进电机目标位置
        self.time = time     # 步进电机从原始位置到达目标位置的时间
        self.remainingDistanceMax = self.pidm.limitTarget  #设置时间过短，最大速度下不能到达指定位置，用于PID调节添加时间的判断条件
    '''
    * functionname : stepMotor
    * para()       :
    * return       : 
    * discription  : 
    步进电机 1600转, 对应一圈, 对应1cm
    10mS, 最大变速100转
    * author       : zhongmingsen
    * date         : 2023-06-14 
    '''
    def stepMotorDstTime(self, dst, time) :
        self.p_dst = dst     # 步进电机目标位置
        self.time = time     
        self.remainingDistanceMax = self.pidm.limitTarget #设置时间过短，最大速度下不能到达指定位置，用于PID调节添加时间的判断条件
        
        timeMin = abs(self.p_dst - self.p_src) / self.pidm.limitTarget * self.ts
        timeMin = round(timeMin, 2)
        if timeMin > time :
            time = timeMin     # 步进电机从原始位置到达目标位置的最小时间
        self.time = time       # 步进电机从原始位置到达目标位置的时间

        timeMax = abs(self.p_dst - self.p_src) / self.pidm.limitOut * self.ts
        timeMax = timeMax / 4
        timeMax = round(timeMax, 2)
        if timeMax < time :
            time = timeMax     # 步进电机从原始位置到达目标位置的最大时间
        self.time = time       # 步进电机从原始位置到达目标位置的时间
        
    '''
    * functionname : stepMotor
    * para()       :
    * return       : 
    * discription  : 
    步进电机 1600转, 对应一圈, 对应1cm
    10mS, 最大变速100转
    * author       : zhongmingsen
    * date         : 2023-06-14 
    '''
    def stepMotorDst(self, dst) :
        self.p_dst = dst     # 步进电机目标位置
        self.remainingDistanceMax = self.pidm.limitTarget #设置时间过短，最大速度下不能到达指定位置，用于PID调节添加时间的判断条件
        
        timeMin = abs(self.p_dst - self.p_src) / self.pidm.limitTarget * self.ts
        timeMin = round(timeMin, 2)
        if timeMin > self.time :
            self.time = timeMin     # 步进电机从原始位置到达目标位置的时间

        timeMax = abs(self.p_dst - self.p_src) / self.pidm.limitOut * self.ts
        timeMax = timeMax / 4
        timeMax = round(timeMax, 2) 
        if timeMax < self.time :
            self.time = timeMax     # 步进电机从原始位置到达目标位置的最大时间
    
    '''
    * functionname : stepMotorFeedbackPwm
    * para()       :
    * return       : 
    * discription  : 
    步进电机 1600转, 对应一圈, 对应1cm
    10mS, 最大变速100转
    * author       : zhongmingsen
    * date         : 2023-06-14 
    '''
    def stepMotorFeedbackPwm(self) :
        if self.feedbackFreq > 0 :
            self.p_src = self.p_src + 1     # 步进电机PWM计数
        elif self.feedbackFreq < 0 :
            self.p_src = self.p_src - 1

    '''
    * functionname : stepMotorPidAdjust
    * para()       :
    * return       : 
    * discription  : 
    * author       : zhongmingsen
    * date         : 2023-06-14 
    '''
    def stepMotorPidAdjust(self) :
        
        try :
            #--------------------------------------------------------------------------------------------------
            #---------------------------PID调节开始--------------------------------------------------------------
            #--------------------------------------------------------------------------------------------------
            ''' 
            |        时间、速度和剩余距离的关系图     
            |            
            |            _ _ _ _ _ _     _ _ _ _ _ _  remainingDistanceMax(v(max))
            |          /            \
            |         /              \  remainingDistance(v(t)) = limitOut * (1 + v(t) / limitOut) * (v(t) / limitOut) / 2
            |        /                \
            |       /                  \
            |      /                    \
            |     /     _ _ _ _ _ _ _    \
            |    /     /       v(t)  \    \
            |   /    /                 \   \
            |  /  /                      \  \
            | //                            \\
            —— —— —— —— —— —— —— —— —— —— —— —— —— —— t
            t0         t1            t2       t3
            t0 -> t1加速阶段, t1 -> t2 平稳运行阶段， t2 -> t3 减速阶段
            '''
            #PID调节步进电机进入减速的条件
            remainingDistance = self.pidm.limitOut * (1 + (abs(self.feedbackFreq) / self.pidm.limitOut)) * (abs(self.feedbackFreq) / self.pidm.limitOut) / 2
            remainingDistance = int(remainingDistance + 0.5)
            #步进电机最大速度也无法达到指定位置，用于增加时间的判断
            if self.remainingDistanceMax < remainingDistance :
                self.remainingDistanceMax = remainingDistance

            #  剩余时间 > 减速时间 and 剩余距离 > 减速距离 
            # （剩余距离 - 减速距离）与（剩余时间 - 减速时间）进行PID速度调节， 对应t0 -> t2 阶段
            #  步进电机PID调节阶段
            if self.time > (abs(self.feedbackFreq) / self.pidm.limitOut) * self.ts and abs(self.p_dst - self.p_src) > remainingDistance :  
                #PID适配减速调节开始
                delat_time = self.time - (abs(self.feedbackFreq) / self.pidm.limitOut) * self.ts
                delat_dis = abs(self.p_dst - self.p_src) - remainingDistance
                self.targetFreq = delat_dis / delat_time * self.ts
                self.targetFreq = int(self.targetFreq)
                if self.p_dst < self.p_src :
                    self.targetFreq = 0 - self.targetFreq
                #PID适配减速调节结束
                #print('--------PID适配加减速调节---------')
                #loggerfc.debug('PID适配加减速调节')
            # 步进电机进入减速调节
            # 剩余时间 > 减速时间 and 剩余距离 > 减速距离  条件不满足， 进入t2 -> t3 减速阶段
            else : 
                #步进电机满足减速调节,减速中 t2 -> t3 阶段
                if abs(self.p_dst - self.p_src) <= remainingDistance :
                    self.targetFreq = 0 #减速中
                    loggerfc.debug('减速中')
                    remainingTime = abs(self.feedbackFreq) / self.pidm.limitOut * self.ts
                    remainingTime = round(remainingTime, 2)
                    if self.time + 2 * self.ts < remainingTime :
                        self.time = remainingTime
                        loggerfc.debug('减速中检查时间, 校正预期时间')
                    #print('--------减速中---------')
                    
                #剩余时间即剩余距离不满足降速达到预期位置，增加时间，以到达位置优先
                elif abs(self.p_dst - self.p_src) >= self.remainingDistanceMax : #时间设置不合理，保持最大速度运行
                    self.time = (abs(self.feedbackFreq) / self.pidm.limitOut) * self.ts
                    self.time = round(self.time, 2)
                    #print('--------添加时间---------')
                    loggerfc.debug('添加时间')
                    #距离和时间的异常变化导致的PID调节开始
                    if self.time != 0 :
                        delat_dis = abs(self.p_dst - self.p_src)
                        self.targetFreq = delat_dis / self.time * self.ts
                        self.targetFreq = int(self.targetFreq)
                        if self.p_dst < self.p_src :
                            self.targetFreq = 0 - self.targetFreq
                        #print('--------PID适配时间调节---------')
                        loggerfc.debug('PID适配时间调节')
                    #距离和时间的异常变化导致的PID调节结束
                else :
                     if self.time >= 4 * motor.ts and 0 == self.targetFreq :
                            delat_dis = abs(self.p_dst - self.p_src)
                            targetAvgFreq = delat_dis / self.time * self.ts
                            self.targetFreq = int(self.targetFreq)
                            if targetAvgFreq > abs(self.feedbackFreq) :
                                if self.p_dst > self.p_src :
                                    self.targetFreq = targetAvgFreq
                                else :
                                    self.targetFreq = 0 - targetAvgFreq
                                loggerfc.debug('减速中检查速度, 校正预期速度')

                            remainingTime = abs(self.feedbackFreq) / self.pidm.limitOut * self.ts
                            remainingTime = round(remainingTime, 2)
                            if self.time + 2 * self.ts < remainingTime :
                                self.time = remainingTime
                                loggerfc.debug('减速中检查时间, 校正预期时间')
                                #print('--------减速中检查速度---------')
                                
                                 
            #如果时间和速度均为0, 且剩余距离大于一个调节最大距离，按最大速度添加一个初始时间
            if self.time == 0 and self.feedbackFreq == 0 and abs(self.p_dst - self.p_src) >= 2 * self.pidm.limitTarget :
                self.time = abs(self.p_dst - self.p_src) / self.pidm.limitTarget * self.ts
                self.time = round(self.time, 2)
                if self.time < 10 * self.ts : #添加的初始时间最少10个调速周期，否则PID调节难以迭代
                    self.time = 10 * self.ts
                #print('-----添加启动时间---------', self.time)
                loggerfc.debug('添加启动时间 %f', self.time)
                    
            #限制目标速度，绘图时便于成比例，PID里有此保护
            if self.targetFreq > self.pidm.limitTarget :
                self.targetFreq = self.pidm.limitTarget
            elif self.targetFreq < 0 - self.pidm.limitTarget :
                self.targetFreq = 0 - self.pidm.limitTarget
            
            #记录步进电机速度跳变LOG
            if abs(self.feedbackFreq - self.feedbackFreqPre) > self.feedbackFreqMax :
                self.feedbackFreqMax = abs(self.feedbackFreq - self.feedbackFreqPre)
            self.feedbackFreqPre = self.feedbackFreq
            
            #PID调节速度，核心调用
            #pid_uk = self.pidm.positionalCal(self.targetFreq, self.feedbackFreq)
            #self.feedbackFreq = int(pid_uk)
            
            pid_uk = self.pidm.incrementalCal(self.targetFreq, self.feedbackFreq)
            #self.feedbackFreq = int(self.feedbackFreq + pid_uk)

            self.feedbackFreq = self.pidm.noPidLimitOutCal(self.targetFreq, self.feedbackFreq)
            
            '''
            if self.targetFreq - self.feedbackFreq > self.pidm.limitOut :
                self.feedbackFreq = int(self.feedbackFreq + self.pidm.limitOut)
            elif self.targetFreq - self.feedbackFreq < 0 - self.pidm.limitOut :
                self.feedbackFreq = int(self.feedbackFreq - self.pidm.limitOut)
            else :
                self.feedbackFreq = int(self.targetFreq)
            '''
                
            # 时间小于最小间隔时，清0
            if self.time > self.ts :
                self.time = self.time - self.ts
                self.time = round(self.time, 6)
            else :
                self.time = 0.0
                if abs(self.feedbackFreq) <= 3 * self.pidm.limitOut :
                    self.feedbackFreq = 0
                    self.targetFreq = 0
                    #求最大距离偏差
                    if abs(self.p_dst - self.p_src) > self.deltaSrcDstMax :
                        self.deltaSrcDstMax = int(abs(self.p_dst - self.p_src))
                        loggerfc.debug('目标位置最大偏差 %f', abs(self.deltaSrcDstMax))

            loggerfc.debug('-- : %f, %f, %f, %f, %f', self.time, self.targetFreq, self.feedbackFreq, self.p_dst, self.p_src)
            
            #--------------------------------------------------------------------------------------------------
            #---------------------------PID调节结束--------------------------------------------------------------
            #--------------------------------------------------------------------------------------------------
            
            self.feedbackFreqList.append(self.feedbackFreq)
            self.targetFreqList.append(self.targetFreq)
            self.srcDisList.append(self.p_src / 100)
            self.dstDisList.append(self.p_dst / 100)
            self.timeList.append(int(self.time * 100 + 0.5))
            self.remainingDistanceList.append(int(remainingDistance / 20))
            self.remainingDistanceMaxList.append(int(self.remainingDistanceMax / 20))

        except : 
            print('stepMotorPidAdjust error')

motor = stepMotor(0.9, 0.01, 0.1, 300, 5, 15)

'''
* functionname : stepMotorTest
* para()       :
* return       : 
* discription  : 
模拟pid调节频率
* author       : zhongmingsen
* date         : 2023-06-14 
'''
def stepMotorTest() :
    motor.stepMotorPara(0,0,0)
    motor.stepMotorPara(0,2*10000,3)
    motor.stepMotorPidAdjust()

'''
* function name : main
* para()        : 
* return        : 
* description   : 用于测试
* author        : zhongmingsen
* date          : 2023.4.1
'''                          
def main(argv=None):
    if argv is None:
        argv = sys.argv
        if len(argv) < 1:
            print('please set dbc file name!')
        else:                                           
            stepMotorTest()
            print('get dbc signal tabel success!')   

if __name__ == "__main__":
    sys.exit(main())

 